#ifndef __TRILAT_H_
#define __TRILAT_H_

#include "../liolib/lioflat.h"

#define ERROR_LEVEL FLT_MAX
#define TEXT_START_X 20
#define TEXT_START_X2 415
#define TEXT_START_X3 870
#define TEXT_START_Y 200
#define TEXT_HEIGHT 39
#define LINE 1.0f
#define DEF_MAX_ITER_COUNT 10000
#define DEF_MIN_ITER_STEP 1e-3
#define DEF_MIN_ITER_GRAD 1e-6
#define DEF_INI_FILE _T("config.ini")
#define ANGLE_X -60
#define ANGLE_Y 0
#define ANGLE_Z -120
#define DIST_X 0.0f
#define DIST_Y -0.25f
#define DIST_Z -3.5f
#define DEF_SECT SECT_R_ACTIVE
#define DEF_CRITIC_SPROD_STOP 0
#define DEF_CRITIC_SPROD_DIR 0
#define DEF_STEP_REDUCT 0.1
#define DEF_SPEED_REDUCT 0.5
#define DEF_STEP_GROW 2
#define DEF_PLAN_TYPE 0
#define DEF_EQUAL_REGION 2
#define DEF_CRITIC_REGION 1
#define DEF_ACTIVE_REGION 0
#define DEF_TARGET_X 0
#define DEF_TARGET_Y 0
#define NEXT_BEACON_ERROR -4
#define NEXT_BEACON_NONE -3
#define NEXT_BEACON_AUTO -2
#define NEXT_BEACON_ACTIVE -1
#define CHNL_INFO_SIZE 6
#define DEF_N 4
#define DEF_K 100
#define DEF_IMG_SIZE 400
#define DEF_AREA_SIZE 100
#define DEF_STDEV_XY 1
#define DEF_STDEV_L 1
#define DEF_STDEV_MP 0
#define DEF_CORREL_XY 0.1
#define SQRT3DIV3 0.57735026918962576450914878050196
#define LOG2PI 1.8378770664093454835606594728112
#define MIN_GRAPH_WIDTH 250.0f
#define MIN_GRAPH_HEIGHT 180.0f
#define CAPTURE_DIR _T("capture")
#define MAPX(n)float(zoom*(n))
#define MAPY(n)float(zoom*(areaSide-(n)))
#define DEMAPX(n)float(((n)-imgRect.x)/zoom)
#define DEMAPY(n)float(areaSide-((n)-imgRect.y)/zoom)
#define VERTEX(x,y) glVertex2f(MAPX(x),MAPY(y))
#define ADDVAL(chnl,value) channels[(chnl)]->addValue(float(value));
#define GPRINT(format, ...) {assert(activeFont); glPrint(activeFont,(format),__VA_ARGS__); activeFont->y+=abs(activeFont->height)+LINE;}

namespace Lio {

class Trilat:public FlatApplication
{
protected:
	enum EnumMaps{
		MAP_NONE,
		MAP_DISTRIB_EST,
		MAP_DISTRIB_NUM,
		MAP_DISTRIB_ERR,
		MAP_CALC_ERROR,
		MAP_TRANS_FACTOR,
		MAP_TRANS_FULL,
		MAP_EST_VAR,
		MAP_EST_VAR_X,
		MAP_EST_VAR_Y,
		MAP_EST_CORREL,
		MAP_PAR_VAR,
		MAP_PAR_ERR,
		MAP_MASS,
		MAP_FULLMASS,
		MAP_GRAD_MASS,
		MAP_ITERS,
		MAP_ITER_MISS,
		MAP_DIFF_VAR,
		MAP_DIFF_CORREL,
		MAP_NUM_VAR,
		MAP_NUM_VAR_X,
		MAP_NUM_VAR_Y,
		MAP_NUM_CORREL,
		MAP_MEAN,
		MAP_MEAN_X,
		MAP_MEAN_Y,
		MAP_EXCESS,
		MAP_EXCESS_X,
		MAP_EXCESS_Y,
		MAP_SKEW,
		MAP_SKEW_X,
		MAP_SKEW_Y,
		MAP_ENERGY,
		MAP_GRAD_ENERGY,
		MAP_MLK,
		MAP_COUNT};
	enum EnumMapTypes{
		MAP_TYPE_NONE,
		MAP_TYPE_DISTRIB,
		MAP_TYPE_EST,
		MAP_TYPE_DIFF,
		MAP_TYPE_NUM,
		MAP_TYPE_ENERGY,
		MAP_TYPE_COUNT};
	enum EnumOrderTypes{
		ORDER_NONE,
		ORDER_RAND,
		ORDER_CIRCLE,
		ORDER_LINE,
		ORDER_COUNT};
	enum EnumModels{
		MODEL_TOF,
		MODEL_S_TOF,
		MODEL_TDOA,
		MODEL_S_TDOA,
		MODEL_S_TDOA2,
		MODEL_ITER,
		MODEL_ITER_A = MODEL_ITER,
		MODEL_ITER_A2,
		MODEL_ITER_G,
		MODEL_ITER_Q,
		MODEL_COUNT};
	enum EnumSections{
		SECT_NONE,
		SECT_R_MAPMIN,
		SECT_R_MAPMAX,
		SECT_R_VARMIN,
		SECT_R_ACTIVE,
		SECT_MIN_MAX,
		SECT_CUSTOM,
		SECT_COUNT};
	enum EnumInfos{		
		INFO_NONE,
		INFO_NUM,
		INFO_COOR,
		INFO_DIST,
		INFO_TRANS,
		INFO_MASS,
		INFO_GRADM,
		INFO_OPT,
		INFO_COUNT};
	enum EnumGraphs{
		GRAPH_NONE=-1,
		GRAPH_SECT,
		GRAPH_DENS,
		GRAPH_CUST,
		GRAPH_ROUTE,
		GRAPH_COUNT};
	enum EnumOpts{
		OPT_NONE,
		OPT_TRACE,
		OPT_DET,
		OPT_COUNT};
	enum EnumMapVars{
		MAP_ARG_POINT,
		MAP_ARG_BEACON,
		MAP_ARG_START,
		MAP_ARG_COUNT};
	enum EnumPlans{
		PLAN_TYPE_SIM,
		PLAN_TYPE_ONCE,
		PLAN_TYPE_ALL,
		PLAN_TYPE_COUNT};
	enum EnumPoints{
		POS_TARGET,
		POS_START,
		POS_END,
		POS_ERRMIN,
		POS_ITER,
		POS_LEVMIN,
		POS_LEVMAX,
		POS_COUNT};
	enum EnumChnl{
		CHNL_NONE = -1,
		CHNL_SECT_ARG,
		CHNL_SECT_VAL,
		CHNL_SECT_LSM,
		CHNL_DENS_X_ARG,
		CHNL_DENS_Y_ARG,
		CHNL_DENS_L_ARG,
		CHNL_DENS_X_EST,
		CHNL_DENS_Y_EST,
		CHNL_DENS_L_EST,
		CHNL_DENS_X_NUM,
		CHNL_DENS_Y_NUM,
		CHNL_DENS_L_NUM,
		CHNL_DENS_X_DIF,
		CHNL_DENS_Y_DIF,
		CHNL_DENS_L_DIF,
		CHNL_CUST_ARG,
		CHNL_CUST_STDEV_EST_X,
		CHNL_CUST_STDEV_EST_Y,
		CHNL_CUST_STDEV_EST,
		CHNL_CUST_CORREL_EST,
		CHNL_CUST_STDEV_NUM_X,
		CHNL_CUST_STDEV_NUM_Y,
		CHNL_CUST_STDEV_NUM,
		CHNL_CUST_CORREL_NUM,
		CHNL_CUST_STDEV_DIF_X,
		CHNL_CUST_STDEV_DIF_Y,
		CHNL_CUST_STDEV_DIF,
		CHNL_CUST_CORREL_DIF,
		CHNL_CUST_VAR_PAR_DIF,
		CHNL_CUST_MEAN_X,
		CHNL_CUST_MEAN_Y,
		CHNL_CUST_MEAN,
		CHNL_CUST_EXCESS_X,
		CHNL_CUST_EXCESS_Y,
		CHNL_CUST_EXCESS,
		CHNL_CUST_SKEW_X,
		CHNL_CUST_SKEW_Y,
		CHNL_CUST_SKEW,
		CHNL_CUST_MASS,
		CHNL_CUST_FULL_MASS,
		CHNL_CUST_MIN_VAR,
		CHNL_CUST_AVG_RAD,
		CHNL_CUST_UX,
		CHNL_CUST_UY,
		CHNL_CUST_UXY,
		CHNL_CUST_LEV_MIN,
		CHNL_CUST_LEV_MAX,
		CHNL_CUST_LEV_MID,
		CHNL_CUST_TIME,
		CHNL_ITER_ROUTE,
		CHNL_ITER_COUNT,
		CHNL_ITER_STEP,
		CHNL_ITER_TRIG,
		CHNL_ITER_ERROR,
		CHNL_ITER_VAR,
		CHNL_ITER_FUNC,
		CHNL_ITER_GRAD,
		CHNL_ITER_GRAD_X,
		CHNL_ITER_GRAD_Y,
		CHNL_COUNT};
	enum EnumScript{
		SCRIPT_NUM_EST,
		SCRIPT_SYM_BEACON,
		SCRIPT_STAT_CHANGE_XY,
		SCRIPT_STAT_CHANGE_L,
		SCRIPT_COUNT};
	enum EnumAction{
		ACTION_NONE,
		ACTION_MAP,
		ACTION_FIT,
		ACTION_SCRIPT,
		ACTION_GRAPH,
		ACTION_PLAN,
		ACTION_COUNT};
	enum ActionErrors{
		ACTION_ERROR_ASSERT = -1,
		ACTION_ERROR_NONE,
		ACTION_ERROR_INTERRUPT,
		ACTION_ERROR_STOPPED,
		ACTION_ERROR_OLD,
		ACTION_ERROR_INIT,
		ACTION_ERROR_COUNT};
	ThreadInfo actions[ACTION_COUNT];
	typedef struct _DATA3D
	{
		float vertex[3],normal[3],rgb[3],hsl[3];
	}Data3D;
	typedef struct _STAT2D
	{
		Point2d<double> mean,stdev,skew,excess;
		double correl,meanL,stdevL,skewL,excessL;
	}FullStat;
	typedef struct _STAT_BEACON
	{
		double devx,devy,corxy,devl,meanl;
	}StatBeacon;
	typedef Point2d<double> Point;
	typedef struct _ITER_INFO
	{
		int count;
		double route,step,var,error,func,trig;
		Point pos,grad;
	}IterInfo;
	typedef struct _ENERGY_INFO
	{
		double e,de,dex,dey,d2edx2,d2edy2;
	}EnergyInfo;
	typedef struct _ERROR
	{
		float x,y,l;
	}Error;
	typedef struct _CHNL_INFO
	{
		CSTR name;
		int x;
		DWORD color;
		GLfloat thick;
		GLushort pattern;
		BOOL hidden;
		CSTR sym;
	}ChnlInfo;
	static ChnlInfo
		chnlInfos[];
	static CSTR
		nameActions[],
		nameOrder[],
		nameMaps[],
		nameModels[],
		nameSect[],
		nameInfos[],
		nameGraphs[],
		nameVars[],
		namePlans[],
		nameOpts[],
		nameShow[],
		nameScripts[],
		nameResults[];
	FlatButton
		bArray,
		bKeepErrors,
		bImage,
		bStat,
		bRandev,
		bMotionEstim,
		bShowDist,
		bShowR,
		bShowIterStart,
		bShowIterEnd,
		bShowTarget,
		bShowIter,
		bShowMin,
		bShowMax,
		bShowErrMin,
		bR,
		bRtoStart,
		bStartToR,
		bIterStart,
		bGoTo,
		bCapture,
		bScript,
		bAddCust,
		bAllCust,
		bClearCust,
		bTrajectCust,
		bFitCust,
		bShowErrors,
		bThreshold,
		bIter,
		bRoute,
		bMapLims,
		bNotify,
		bParabEstim,
		bSave,
		bLoad,
		bBrowse,
		bDump,
		bActiveUpdate,
		bActiveSwap,
		bForceM,
		bForceP,
		bForceC,
		bLSM,
		bWinSize,
		bShow3D,
		bMassMax,
		bPlan,
		bShowFirstPass,
		bConsole;
	FlatLabel
		lActive,
		lArea,
		lImage,
		lModel,
		lMap,
		lInf,
		lSup,
		lOpt,
		lOrder,
		lSamples,
		lCount,
		lStdev,
		lStdevXY,
		lStdevL,
		lMeanL,
		lCorrelXY,
		lCust,
		lActiveCoor,
		lActiveStdev,
		lActiveStdevX,
		lActiveStdevY,
		lActiveStdevL,
		lActiveMeanL,
		lActiveCorrelXY,
		lShow,
		lSection,
		lMinIterStep,
		lMaxIterCount,
		lMinIterGrad,
		lMotion,
		lStepParams,
		lGraph,
		lMotionEqual,
		lMotionCritic,
		lMotionActive,
		lSpeedReduct,
		lTarget,
		lErrDist,
		lForce;
	FlatChoice
		chMap,
		chActive,
		chFormat,
		chModel,
		chSection,
		chInfo,
		chVar,
		chOpt,
		chPlan,
		chScript,
		chGraph,
		chOrder;
	FlatText
		tbCount,
		tbArea,
		tbImage,
		tbStdevXY,
		tbCorrXY,
		tbStdevL,
		tbMeanL,
		tbSamples,
		tbSup,
		tbInf,
		tbRx,
		tbRy,
		tbErrDist,
		tbIterStartX,
		tbIterStartY,
		tbMapMin,
		tbMapMax,
		tbMinIterStep,
		tbMaxIterCount,
		tbMinIterGrad,
		tbCapture,
		tbCompr,
		tbFile,
		tbDump,
		tbActiveX,
		tbActiveY,
		tbActiveStdevX,
		tbActiveStdevY,
		tbActiveStdevL,
		tbActiveMeanL,
		tbActiveCorrel,
		tbPoints,
		tbStepReduct,
		tbStepGrow,
		tbLSM,
		tbWinSizeX,
		tbWinSizeY,
		tbMotionEqual,
		tbMotionCritic,
		tbSpeedReduct,
		tbMotionActive,
		tbTargetX,
		tbTargetY;
	FlatGraph fg;

	volatile HANDLE active_thread;

	FullStat numStat,estStat;
	MatrixInvertor invertF;
	Matrix
		mA,mAt,mX,mD,mEa,
		mA0,mInvA0,
		mInvA,mAquad,mDa,
		mPx,mPtmp,
		mF,mV,mU;
	MATRIX D,Px,Ptmp,Aquad,Da,X,invA0,Ea,A0,A,invA,At,F,invF,V,U;
	Channel *channels[CHNL_COUNT];
	List<Point> trajectory;
	typedef List<Point>::Object PointObject;
	List<IterInfo> route,tmpRoute;
	typedef List<IterInfo>::Object IterObject;
	Point	R,estR,*B,*gradM,
			custStart,custEnd,
			estVar,minVar,
			orderStart,orderEnd,
			sectStart,sectEnd,mark,
			p[POS_COUNT];
	Point2d<int> oldMousePos;
	Table<Error> tErrors;
	Table<double> tGraphBuffers;
	Vector<Matrix> vJ,vP;
	Vector<double> vR2,vL,vLe,vLevels,vM,vSectCoefs,vO;
	Vector<float> v_mp_errors;
	Vector<Error> vDistrib;
	Vector<Data3D> vBuff3D;
	Vector<Point> vB,vGradM;
	StatBeacon *stats;
	Vector<StatBeacon> vStats;
	Matrix *mJ,*mP;
	Data3D	*buff3D;
	IterInfo iterOut,iterMass,iterMotion;
	float	zoom,pointSize,lineGran,lineMin,lineMax,
			angleX,angleY,angleZ,distX,distY,distZ,*mp_errors,
			LightAmbient[4],LightDiffuse[4],LightPosition[4];
	int		map,show,cols,rows,active,section,points,
			N,K,areaSize,areaSide,imgSide,model,order,
			mapErrorColor,mapThresColor,trajectColor,routeColor,
			graph,mapArg,mapType,count3DWeb,step3DWeb,
			maxIterCount,itersMmax,itersMin,iterIndex,
			action_result,threads_count;
	double	stdevXY,stdevL,meanL,correlXY,
			estCovar,FGW,detfactor,iterErrRatio,
			levmin,levmax,levmid,mapmin,mapmax,ind_min,ind_max,
			progress,kSample,kBeacons,actionTime,
			thresInf,thresSup,maxGradM,maxF0,
			**buff,detU,bPosDevMod,recalc_time,
			maxStepMass,stepStart,stepReduct,stepGrow,
			lt,la,rc,sr,k_obst,minIterStep,minIterGrad,
			criticSprodStop,criticSprodDir,iterErrSquare,
			*O,*R2,*levels,*Lr,*Le,*M,Ux,Uy,Uxy,custArg;
	Error	**errors,*distribs;
	BOOL	randev,optim,processing,
			mode3D,info3D,frame3D,box3D,xyz3D,values3D,web3D,onlyWeb,smooth3D,light3D,color3D,minMax3D,
			parabEstim,motionEstim,INVA_OK,cog_ok,parabOK,parabPending,keepErrors,
			drawCond[POS_COUNT],statProcess,mapArgChanged,mapChanged,mapSection,mapNoArg,useCustArg;
	long __stopAtLine;
	DWORD pending_action;
	Image image;
	Rect<float> imgRect;
	Font fontSmall,fontBig,fontMatrix,*activeFont;
	CriticalSection run_sect;
	DisplayList list3D;
	String graphText,graphText1,currentDir;
	PrecisionTimer actionTimer;
	Transform trans;

	BOOL create();
	BOOL glCreate();
	//void glDestroy();
	void destroy();
	BOOL listen(WORD id, WORD code, const void *info);
	void update()
	{
		setUpdated(processing&&bNotify.checked);
	}

	void init();
	void initUI();
	void init3D();
	void compile3D();
	void set3D(BOOL value);
	void get3DWebStep(BOOL searchDown);
	BOOL dump(FILE *file);
	void dump(CSTR fileName);
	void draw3D();
	void draw2D();
	void reshape();
	BOOL setArray(int newBeacons, int newSamples = -1, BOOL keep = FALSE);
	void setModel(int newModel);
	void updateModel();
	BOOL setImage(int newImageSide, int newAreaSide);
	void adjustWindow();
	void setStat(	double newStdevXY,
					double newCorrelXY,
					double newStdevL,
					double newMeanL,
					BOOL newRandev);
	BOOL setOptim(int newOptim);
	void capture();
	inline BOOL getLevel(double x, double y, double &level);
	void updateSectionMark();
	void updateSection();
	void updateActiveInfo();
	void setActiveInfo(double x, double y, double devx, double devy, double corxy, double devl, double meanl);
	BOOL setActiveBeacon(int newActiveBeacon);
	void showPos();
	void reset();
	void allowParabEstim(BOOL newParabEstim);
	void allowMotionEstim(BOOL newMotionEstim);
	double getMassGrad(Point *grad, int beacon = -1);
	double getMassMax(int beacon = -1, Point * start = NULL, BOOL routing = FALSE);
	void setMotionTarget(double x, double y);
	void setMotionParams(	double equalRegion,
							double criticRegion,
							double activeRegion,
							double speedReduct);
	void buildStat();
	void updateStats(BOOL updateErrors = TRUE);
	void updateStat(int beacon, BOOL updateErrors = TRUE);
	void updateOpt();
	void prefetch();
	void getCOG();
	void addCust();
	void clearCust();
	double getParabVar(const Point &pos){return getParabVar(pos.x, pos.y);}
	double getParabVar(double x, double y);
	void setThreshold(double inf, double sup);
	BOOL setGraph(int newGraph, int newPoints);
	BOOL setMapLims(double min, double max);
	void adjustGraph(	int gridx, int gridy,
						int precx, int precy,
						double margex, double margey,
						double scalex, double scaley,
						BOOL smooth, BOOL symAuto,
						BOOL log, BOOL exposant,
						BOOL toolbox);
	void showMapLims();
	BOOL input(BYTE type);
	BOOL getStatEst(double x, double y, Point &out, Point &var, double &covar, IterInfo &iterOut, BOOL routing);
	BOOL getStatNum(double x, double y, FullStat &statXY, BOOL calcDistrib);
	void setOrder(int newOrder);
	BOOL locateBeacons();
	void setMap(int newMap, int newArg);
	BOOL setSection(int newSectId, int lsmCount);
	BOOL getParabCoef(BOOL forced=FALSE);
	void recalc(BOOL custOK = FALSE);
	void setIterParams(double minStep, int maxCount, double minGrad, double stepReduct0, double stepGrow0, double errDist);
	BOOL load(CSTR filename);
	BOOL save(CSTR filename);
	BOOL invMatrixA()
	{
		transMatrix(A,At,rows,cols); // A'
		mulMatrix(At,A,Aquad,cols,rows,cols); // Aq = A'A
		detfactor=(detMatrix(Aquad,cols,cols)); // |Aq|
		INVA_OK=invMatrix(Aquad,Da,cols,DBL_EPSILON);
		if(!INVA_OK) // Ainvq = Aq^-1
			return FALSE;
		mulMatrix(Da,At,invA,cols,cols,rows); // Ainv = Ainvq * A'
		return TRUE;
	}
	void stopProcessing()
	{
		processing=FALSE;
		if(parabPending)
		{
			parabPending=FALSE;
			getParabCoef();
		}
	}
	void swap(int beacon);
	DWORD run(__int64 action);
	DWORD run(const ThreadInfo *info)
	{
		__int64 action = info - actions;
		if(action >= ACTION_NONE && action < ACTION_COUNT)
			return run(action);
		return FlatApplication::run(info);
	}
	void stop(BOOL wait = FALSE, long line = __LINE__);
	BOOL start(DWORD action)
	{
		return FlatApplication::start(actions+action);
	}
	void startMap();
public:
	Trilat()
	{
		Clear(actions);
		for(int i=ACTION_NONE;i<ACTION_COUNT;i++)
		{
			ThreadInfo *info = actions + i;
			info->name = nameActions[i];
			info->priority = THREAD_PRIORITY_NORMAL;
			info->caller = this;
		};
	}
};

}; //namespace Lio

#endif // __TRILAT_H_